/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file swiftpilotParser.cxx
	\brief class implemenation for swiftpilotParser

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "swiftpilotParser.h"


  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
    bst::Telemetry_t  telemetry;
    bst::Limits_t     limits;
    bst::PID_t        control_loops[NUM_CTRL_LOOPS];
    bst::WayPt_t      flight_plan[MAX_WAYPOINTS];
    Packet packet;
  /*<-End Global Variables-->*/

SPParser::SPParser(String name) : ThreadedPipe(name)
{
  new_sensors = false;
}

void SPParser::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;
	uint8_t c;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("SPParser::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {
        // Read the SwiftPilot Packets
        for(size_t ind=0; ind<n; ind++) {
          c = (char)fifo_msg.data[ind];
          readPacket(c);
        }
      }
    }
    if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs) && new_sensors) {
      GPSLLA_t * dataPtr = (GPSLLA_t *)CMD_DATAPTR(fifo_msg.data);

      CMD_CMD((fifo_msg.data)) = CMD_GPS_LLA;
      CMD_NUM((fifo_msg.data)) = sizeof(GPSLLA_t);
      dataPtr->lat = telemetry.sensors.gps.latitude;
      dataPtr->lon = telemetry.sensors.gps.longitude;
      dataPtr->alt = telemetry.estimate.altitude;
      CMD_CSUM(fifo_msg.data) = 0;
      CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

      fifo_msg.size = CMD_SIZE(fifo_msg.data);

      Client * a_client;
      Client * me = shmPtr->clients.getClient(0);
      int i=0;
      while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_LLA,i)) != NULL)
      {
        fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
        fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
        writeFifo(INBOARD_TX);
        i++;
      }
      new_sensors = false;
    }
  }
}

void SPParser::readPacket(char c)
{
  // Build up the SP Packets and save them to the telemetry struct
  if(packet.isValid(c)) {
    uint8_t   temp_waypoint_num = 255;
    uint8_t   num_wypts = 0;
    PID_t     temp_pid;
    Surface_t temp_surface;
    Command_t temp_command;
    WayPt_t   temp_waypt;

    switch(packet.getType()) {
      case TELEMETRY:
        packet.getData((uint8_t *)&telemetry);
        break;
      case SENSORS:
        packet.getData((uint8_t *)&telemetry.sensors);
        new_sensors=true; //FIXME
        break;
      case STATE:
        packet.getData((uint8_t *)&telemetry.estimate);
        break;
      case CONTROL:
        packet.getData((uint8_t *)&telemetry.control);
        break;
      case COMMAND:
        packet.getData((uint8_t *)&temp_command);
        break;
      case SURFACES:
        packet.getData((uint8_t *)&telemetry.surfaces);
        break;
      case LIMITS:
        packet.getData((uint8_t *)&limits);
        break;
      case CONTROL_LOOPS:
        packet.getData((uint8_t *)&temp_pid);
        memcpy(&control_loops[temp_pid.id],&temp_pid,sizeof(PID_t));
        break;
      case FLIGHT_PLAN:
        packet.getData((uint8_t *)flight_plan);
        break;
      case DEL_FLIGHT_PLAN:
        packet.getData(&temp_waypoint_num);
        break;
      default:
        printf("UNHANDLED PACKET\n");
    }
  }
}

void SPParser::sendLimits() {
  Packet packet(LIMITS);
  packet.setData((uint8_t *)(&limits), sizeof(bst::Limits_t));
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::sendCommand(CommandID_t id, float value) {
  Packet packet(COMMAND);
  Command_t temp_command;
  temp_command.id = id;
  temp_command.value = value;
  packet.setData((uint8_t *)(&temp_command), sizeof(Command_t));
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::sendWayPoint(uint8_t num) {
  Packet packet(FLIGHT_PLAN);
  packet.setData((uint8_t *)(&flight_plan[num]), sizeof(WayPt_t));
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::delWayPoint(uint8_t num) {
  Packet packet(DEL_FLIGHT_PLAN);
  packet.setData(&num, sizeof(uint8_t));
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::clearWayPoints() {
  Packet packet(FLIGHT_PLAN);
  packet.setData(NULL, 0);
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::requestControl() {
  Packet packet(CONTROL);
  packet.setData(NULL,0);
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::requestLimits() {
  Packet packet(LIMITS);
  packet.setData(NULL,0);
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::requestControlLoops() {
  Packet packet(CONTROL_LOOPS);
  packet.setData(NULL,0);
  //sock->write((char *)packet.getPacket(),packet.getSize());
}

void SPParser::requestFlightPlan () {
  Packet packet(FLIGHT_PLAN);
  packet.setData(NULL,0);
  //sock->write((char *)packet.getPacket(),packet.getSize());
}
